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ABSTRACT 


Path Planning is an intricate part of the navigation 
function of any vehicle traveling between two points in 
Space. In an autonomous underwater vehicle, a trajectory 
may be planned between two points using the optimization 
techniques of ADS (Advanced Design Synthesis) coupled to a 
motion analysis routine, DSL (Dynamic Simulation Language). 
The problem is posed as a two-point-boundary-value problem 
with initial states and desired final states known, as well 
as a final time specified. The objective function is 
minimized in the form of a quadratic regulator for the 
purpose of conserving the vehicles energy supply. An 
obstacle in the dive plane (X-Z plane) is introduced and 
successfully avoided using the constrained optimization 
technique. The use of optimization is proven as a feasible 
method for successfully planning the trajectories of 
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THESIS DISCLAIMER 


The reader is cautioned that computer programs developed 
in this research may not have been exercised for all cases 
of interest. While every effort has been made, within the 
time available, to ensure that the programs are free of 
computational and logic errors, they cannot be considered 
validated. Any application of these programs without 


additional verification is at the risk of the user. 
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Path planning is the function provided by an intelligent 
system which determines a safe, collision free trajectory of 
travel between two points, a start point and a target point, 
for a specific time lapse. This has been performed 
successfully for a number of land vehicles by many different 
techniques [Refs.1-3]. Several classes of techniques 
available today include graphical search methods [Refs.4-8], 
artificial potential field methods [Refs. 9-13] and optimal 
control theory (Refs. 14,15}. The approach taken in the 
present work falls in the optimal control theory class. 
Here, a sSingle-valued penalty function was used to evaluate 
a path between the two positions. The "best" path was then 
found by minimizing the penalty. The mathematics of this 
approach was fairly intense, but the advantage is that 
optimization in space and time were accomplished 
simultaneously. 

Path planning is an open loop control problem that 
optimizes the control vector U to produce a best state 
trajectory, X, based on the penalty function, which is also 
referred to as the objective function. 

This baseline study of optimal control theory as applied 
to path planning was directed toward being utilized in an 


Autonomous Underwater Vehicle (AUV) testbed at the Naval 


Postgraduate School. At the time of this writing an AUV was 
being designed to operate in the ocean environment 
untethered from any command structure or man-in-the-loop 
system. To operate on its own the AUV must have, in 
addition to many other self sustaining systems, a system or 
means to plan its track from the present position to a 
target position some distance away and at some specified 
time in the future. Figure 1.1 shows the states that are of 
major concern when dealing with motion in the vertical 
plane. The x-state 1s the distance the vehicle travels in 


the hoemazontal direction. The z-state is the distance 
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Figure 1.1 Dive Plane State Relationship 


the vehicle travels vertically. The theta-state is the 
pitch angle of the vehicle, in radians. As mentioned 
earlier, the objective was to have this path be safe, 
collision free and energy efficient. Energy utilization was 
of key importance since the power source was assumed to be 
limited to a finite source onboard. The path planner was 
a to operate ina large area transit mode where the 
speed would be moderate and the obstacle environment sparse. 

The Automatic Design Synthesis (ADS) FORTRAN program 
(Ref. 16] was utilized to accomplish the optimization 
when coupled to the Dynamic Simulation Language (DSL) 
(Ref.17]. The coupled package was referred to by the 
acronym ADSL. The IBM 3033 mainframe computer system 
provided the environment to run the programs. ADS provided 
an options package that allowed for the utilization of all 
known methods available to do numerical optimization. 
DSL provided selection of integration methods necessary 
to integrate the equations of motion (state equation) and 
determine the "best" path. The amount of time necessary to 
compute this path was also a factor worthy of consideration 
as it was desired to have the path planner perform at, or 
near, real time. It was assumed that these algorithms would 
later be hard-wired into a system for use in an underwater 
vehicle. 

The approach taken was to initially utilize ADSL on an 


unconstrained nonlinear SISO (Single input single output) 


open loop minimization problem. The problem was chosen for 
its similarity to the AUV nonlinear dynamics structure. 
This problem allowed for initial studies on effectiveness of 
integrators, time interval step sizes, strategy and 
optimizers. This procedure was then applied to a simplified 
linear model of dive plane motion with a corresponding 
multiple input multiple output (MIMO) model. Comparison of 
results with the full-scale nonlinear model developed by Lt. 
Boncal followed [{Ref.18]. The "best" path was determined in 


all cases. 


II. SISO MINIMIZATION PROBLEM 


he bai PROBLEM 

The SISO minimization problem was based on the 
continuous time problem of example 10.2-6 of Sage 
[Ref.19:p.313]) (see Appendix A). This problem was chosen 
because of its similarity to the structure of the nonlinear 
dynamics of an underwater vehicle which will be discussed in 
the next part of this study. Sage arrived at the solution 
by using a continuous time analytical method and a 
gradient-in-function-space technique. Optimization was 
reached through an analytical equation that gave the results 
plotted in Figure 2.1 for the control variable, and Figure 
2.2 for the state variable. 

In the present work a discrete time solution was chosen 
that gave a set of single-valued control inputs evenly 
distributed over a specified number of delta-time steps. 
The optimum open loop control vector therefore appeared as a 
stepped function in time, which well mimics a computer-based 


eentrollereoutput. 


B. QOBSECTIVE FUNCTION 
The objective function was a one dimensional quadratic 


performance index of the form: 
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2.1 Control Optimization, after Sage [Ref. 19] 
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Figure 2.2 State Optimization, after Sage [Ref. 19) 


where: 


U = the control variable; and 


ms 
ll 


the output state. 


In actuality the computer saw the objective function as: 


ye 2 
Wea) (xe + UL) AT (2-2) 
if it 
is 
where: 
N = the number of discrete time intervals, each 


of which corresponds to a single design 
variable (DV) in the control variable history. 


Figure 2.3 shows the relationship between the design 
variables (DV), the time intervals, the discrete control 


variables (also DV) and the continuous control signal. 


C. DESIGN VARIABLES 

It was desired to chose a number of DV's in U that would 
yield a close approximation of the continuous time solution. 
Table 1 shows the mainframe computer run times and error 
data for cases where 100, 50, 20, and 10 time intervals were 
established between start time and finish time (0 to 1.0 
sec). The rectangular integration method with .005 second 
step size was used for all cases. 

Based on the extended virtual machine time (VM) on the 


NPS IBM mainframe computer for the 100 and 50 time interval 


—0.4 


DiscretGwe. 


TABLE 1 
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Number 
of 
Design Run Ob jec- 
Variable Time tive 
SAGE - 4.4859 
100 8.38 4.4930 
50 4.96 4.4934 
20 3.30 4.4920 
10 2.46 4.4917 


Objec- 
tive 
ERrRor 


16% 
17% 
14% 


. 13% 


X-State 
Final 

Value 

0.83 327 
0.84347 
0.84339 
0.83680 


0.83385 





X-State 
Final 
Value 
Er rem 


- 42% 


-07% 


problems and the fact that the results do not show any 
improvement in the solution, these two intervals were not 
considered as viable solutions. The 20 and 10 interval 
results were further analyzed to determine the number of 
design variables and also the recommended time step size to 
be used during the integration of both the state xX and 
objective function J. First, however, the type of 


integration method needed to be determined. 


D. INTEGRATION METHOD 
DSL (Dynamic Simulation Language) has a variety of 
integrators to use for any type of problem. Table 2 gives 


the available methods in three groups. 


TABLE 2 


DSL INTEGRATION METHODS 


Fixed Step Methods Order 
RECT Rectangular 0 
TRAPZ Trapezoidal 1 
SIMP Simpson's 3 
RKSFX Runge-Kutta 4 

Variable-step Methods 
RKS Runge-Kutta 4 
RK5 Runge-Kutta 

Variable-step, variable-order Methods 
ADAMS Adams-Moulton Loa? 
STIFF Gear, full Jacobian 1-5 
BSTIFF Gear, Banded Jacobian 1-5 


The fixed step methods are easily controlled using the 
DELT parameter that tells DSL the delta time step size to 
use in the simulation. The rectangular method is the 
Simplest, and the Runge-Kutta the more complex of the fixed 
step integration methods. Since the design variables were 
actually based on fixed time increments, the variable step 
methods were more difficult to utilize. However, with the 
additional DSL parameters of DELMAX and DELMIN these methods 
were also utilized to conduct the simulation comparisons. 
Comparisons using the 10 and 20 time intervals were made 
with the RECT, RKSFX, RK5, and ADAMS methods. 

Results found in Table 3 give the run times’ and 
accuracies of both the objective function and the value of 
the X state at 1.0 second, the final time. The results 
show that the fixed step rectangular integration method was 
the overall most accurate method when considering both 
objective function and final X state after the 1.0 second 
interval. Also, as could be expected, the run times 
increased with complexity of the integration routines. To 
ensure that the variable step size methods covered each 
design variable time increment, the DELMAX and DELMIN values 
were set equal to the DELT parameter. The accuracies of the 
objective function were very good for the higher order 
integration methods; however, their accuracies for the final 


X state were worse than for the simple rectangular method. 
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When run time was considered as well, the "best for 
least" concept was used and the rectangular integration 


routine was selected as the best overall choice. 


E. INTEGRATION STEP SIZE 

The integration algorithms required a finite amount of 
time to run depending on step siZe. As the step size 
decreased, the time to complete the integration of the 
objective and state equations increased. Also, the accuracy 
of the integration result increased to a point then remained 
relatively constant. Table 4 provides tabulated data on 
integration step size, time requirements and accuracies for 


the two feasible time interval solutions. 


TABLE 4 


INTEGRATION RESULTS 


Time 20 Bier or 10 PiEror Sage 
Category Intervals DV's % DV's % Result 
Run “OOm 11.36 = 7. Gu = = 
~005 3 90) = 2.46 — 
Time a0 E 2 . ae. = Lida? - 
(Sec) .05 1.3@ - 1 wa - 
eOOUL 4.4877 .04 4.4874 . CEs 4.4859 
Objective ~005 4.4920 -14 4.4917 . & 
oe) di 4.4975 26 4.4972 aa 
Value 0S 4.3463 dees 4.5463 ie, 3 
-O001 -84476 1.4 218). ds. OZ 83327 
Final -005 -83680 .42 -83385 -07 
x -O1 8226/8 «78, 82388 lL.2is 
State -05 -/i0e@o LIvs -73648 11.6 
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Results revealed that for 10 time intervals the best 
performance was found at the 0.005 second integration time 
step size. The run time was approximately equivalent to the 
20 interval 0.01 second integration step size. The key to 
performance was the accuracy of the output state with 
respect to time. Figure 2.4 superimposes the Sage solution 
for the X state with the X state for the 20 interval 0.005 
step size and the 10 interval 0.005 step size. They are all 
very close. However, the control prediction becomes the 
discriminating factor. Here, the 10 interval 0.005 step 
size shown in Figure 2.5 is the best choice for duplicating 
the control variable. 

Therefore the overall "best for least" result was found 
to be the 10 time intervals, 0.005 second integration step 
size in conjunction with the rectangular integration 


algorithm. 


F. OPTIMIZATION 

The ADS (Advanced Design Synthesis) program provided the 
means to combine a wide variety of optimization concepts so 
that the best combination could be selected to solve the 
problem. The concepts were divided into three basic levels 
for solving the type of minimization problem described in 
Reference 20, page 1. These three levels were the strategy 
level, the optimizer level and the one dimensional search 
level. Table 5 lists the levels and the algorithms 


available. Reference 20, page 5 provides a matrix which 


13 


10 










LEGEND 
SAGE 
© 10 INTERVAL(.005 

/ 4 20 INTERVAL(.005 
= A 
<{ 
E bal 
+ 
P< 

2 zs . 

© 


O:0-"Ohte= Of OF OM TOR OE or OS "Ue 
TIME 


Figure 2.4 X-state Values for Discrete Controls 


14 


CONTROL U 


= 06 


-0.4 


a. se 


Oa 


- Ort 


© 
CS 


ODOR Ae Oc 


Pargwre 2.5 


Uae O46 0.0. 0.6 0.7 


TIME 


Discrete Control Results 


ju S 





TABLE 5 


ADS OPTIONS 


Strategy (ISRRAT) 


0 


i 


None 

SUMT, Exterior Penalty Function 
SUMT, Linear Extended Interior 

SUMT, Quadratic Extended Interior 
Cubic Extended Interior 

Augmented Lagrange Multiplier Method 
Sequential Linear Programming 
Method of Centers 

Sequential Quadratic Programming 


Sequential Convex Programming 


Optimizer (IOPT) 


i 


2 


Fletcher-Reeves 
Davidon-Fletcher-Powell (DFP) 
Broydon-Fletcher-Golfarb-Shanno (BFGS) 
Method of Feasible Directions 


Modified Method of Feasible Directions 


dimensional Search (IONED) 

Golden Section Method 

Golden Section and Polynomial 
Polynomial Interpolation, bounded 
Polynomial Extrapolation 


Golden Section Method 
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TABLE 5 (CONTINUED) 


6 - Golden Section and Polynomial 
7 - Polynomial Interpolation, bounded 


8 - Polynomial Extrapolation 


gives the meaningful combinations of algorithms to be used 
for various types of problems. In summary, the two major 
types of problems are classified as constrained or 
unconstrained minimization. The Sage problem was 
unconstrained so strategies 1 through 5, optimizers 1 
through 3 and one dimensional search methods 1 through 4 
were all possible selections. For a constrained problem, as 
for the AUV model, strategies 6 through 9, optimizers 4 and 
5 and one dimensional search methods 5 through 8 were 
possible. If the strategy is eliminated, the solution 
process starts with the optimizer. 

Olson [Ref.21:pp. 27 =3.20) Summarized the options 
avallable and made further reference to Vanderplaats 
(Ref.22] for additional details concerning these methods and 
algorithms. The material will not be repeated here. 

The strategy, as stated earlier, was optional for all 
problems; however, an optimizer and one dimensional search 
algorithm were required. 

In determining the combination of strategy, optimizer 
and one dimensional search method that was the most accurate 


for solving the Sage unconstrained problem, all the possible 
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combinations were run. It was quickly determined that the 
strategy option when invoked caused the execution time and 
the number we calls to ADS to increase. The solutions were 
not as accurate as those without a strategy invoked. It was 
determined that the best optimizer proved to be the 
Fletcher-Reeves conjugate direction method (IOPT = 1). This 
method was a Simple modification to the first-order method 
of steepest descent. It involved gradient information 
normally supplied by using finite difference computations 
Ref <220: DaeeciS de The conjugate direction method — 
picked search directions that were conjugate by definition 


of the search direction equation: 


xl = x9 - oVF(X°) (2-3) 


ms 


where: 
xl = new design variable vector 
x9 = previous design variable vector 
a = scalar parameter 
7 F(x) = gradient of F(X). (Ref. 22:ipe 74) 


The one dimensional search algorithm that provided the 
best results with the optimizer was the method of polynomial 
extrapolation (IONED = 4). The graphical results of Figures 
2.4 and 2.5 were those using the above optimizer and one 


dimensional search combination. 
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III. PATH OPTIMIZATION FOR A LINEAR MIMO PLANT 


A. MIMO PLANT FORMULATION 

With the optimization method tested on a_ SISO 
unconstrained problem, and realizing computation times were 
Satisfactory for a near real time solution, the method was 
applied to a linear MIMO plant. Specifically, a linearized 
model of an under-water vehicle restricted to dive plane 
motion was developed. The development followed the 
procedures specified in [Ref. 23: p. 476) with the 
assumptions that: 


He) the vehicle equilibrium condition was along a 
straight line path; 


isl.) it traveled at constant speed; 

11i) it moved ina fixed horizontal direction, and 

iv) it had xg = 0. (xg is the distance the body 
fixed coordinate system is from the actual 
center of gravity of the vehicle in the x- 
direction.) 

The standard six-degree-of-freedom dynamic equations of 
motion of a submarine developed by Gertler and Hagen 
(Ref.24] and revised by Feldman [Ref.25] were the nonlinear 
equations that were linearized. Due to the above assumption 
for motion restricted to the vertical plane, the axial force 
equation (X) was decoupled from the pitch moment equation 


(M) and the normal force equation (2). The equations 


Simplify to the following: 
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“2, w+(m-Z.)w -(Z 4m )q -2aq = 24,08 +2,_,DB (3-1) 
1 rere ret ' eI ro ' ' ' ' 
“Mw “Mew “Mig +(I,-M2)q -M,9 =M,_DS +M3_DB (3-2) 


The additional coupling equation linking the forward motion 


speed to the problem is: 
Zi = ye ae (St3) 


where el 1S a constant forward speed. The model is 
completely nondimensionalized by the vehicle's length of 
approximately 17 feet, and a velocity of six feet per 
second. The equations were modelled in state space form 
with the four states being: 

thetadot = rate of change of pitch 


theta = pitch angle in radians 


W rate of change of depth 


Z depth (positive going down). 

A DSL simulation program (STSPACE3 DSL in Appendix B) 

verified that the model was working properly. The coupling 

of ADS and DSL followed with path optimization as the goal. 
The vehicle from which this model is derived has coupled 

stern and bow planes. That is, given a stern plane 

deflection, the bow planes deflect opposite in direction 


with a given proportionality constant. This aids in 


maneuvering the vehicle. With this coupling, the actual 
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control matrix was a scalar, associated with the stern plane 
deflection only. Controllability was checked and the plant 
was verified as controllable. However, in the following 
discussion, the planes were decoupled so that two separate 
control inputs were optimized over the specified length of 
time to perform any dive maneuver. This was done for 


additional controllability. 


B. DEFINING THE PROBLEM 

When an underwater vehicle travels from one position to 
another, the path upon which it traverses is a smooth 
continuous path that does not have discontinuities. If the 
vehicle is stable, the trajectory is predictable and it may 
be expected that a path planner using optimization 
techniques could find the "best" path from the start 
position to the end. Further, while finding the best path, 
all stationary obstacles must be avoided, if feasible. 

The sample problem was formulated for a maneuver in the 
vertical plane, a one unit depth change (17.425 feet) in 20 
seconds (t' = 7.0), with the vehicle at an initial forward 
velocity of six feet per second (one unit of nondimensional 
velocity), without obstacles. The initial conditions were 
associated with the vehicle traveling at constant horizontal 
motion at an arbitrary depth (zero in the figures). At 
time equal to zero (t' = 0.0) the coordinate system was 
positioned at the vehicles center of gravity. The final 


state was to occur at time equal to 20 seconds (t' = 7.0) 


Zl. 


with Z = 17.425, theta = 0.0, and the control surfaces at 
their equilibrium positions. In other words, the vehicle 
was to be at the desired end depth at zero pitch and with 
control surfaces at the neutral position so that immediately 
after 20 seconds the vehicle would continue to travel along 
the desired depth. Before discussing the optimization 
techniques required to solve such a problem as depth change, 


the objective function and approach taken will be discussed. 


C. THE OBJECTIVE FUNCTION 
For the linear plant model given by ae system of 


equations in the form 
x=Ax + Bu, (3-4) 


the desired function to minimize is an error function of 


the form 
a 
[ (x~(t) - x(t) ]7Qfx_e(t) - x(t) ]dt Gea 
Phe . os 


In addition, since power consumption 1s paramount when 
dealing with self contained underwater vehicles, a 
Minimization of the control energy should be included. The 
following equation forms a power penalty on the control 
vector u: 


~es 


T 
( (ace? Geepeyayere (3-6) 
0 awe rae re 
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So, the combined objective function that should be minimized 
is: 


ar 
(< iraaa Olea Gt msc) +qa(t) 4Ru(t) pat (3-7) 


This is the quadratic performance index presented by Ogata. 
BRehe26:p./753 ] 

For a depth change maneuver all states except the 
position states should be zero at the end state. In a 
strict X-Z plane maneuver with the X state decoupled, the 
final Z position will have a value other than zero (say, 2Z); 


hence, the "f" subscripts in the equation above. 


D. THE APPROACH 
Four combinations of end time treatments are thus 
possible to solve this two point boundary value problen: 


1) Minimize the quadratic performance index by gain 
adjustment without end constraints. 


2) Use the ADS defined end constraints and minimize the 
objective. 


3) Combine ADS defined end constraints with penalty 
functions added to the objective. 


4) Minimize the objective combined with penalty 
functions on the desired terminal conditions, 
without ADS end constraints. 

The results of each combination is presented in the 
following discussion and graphically in the accompanying 


figures. A discussion of the accuracy and timing will 


follow at the end of the chapter. All methods required the 
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use of side constraints to limit the bow and stern plane 
deflections. 
ly, wSide Constwammnts 

After considering the actual movement oof an 
underwater vehicle through the water, it was realized that 
several restrictions needed to be placed on the vehicles! 
maneuvering surfaces. The maximum stern plane deflection 
was limited to no more than a 10 degrees up or down angle 
from the equilibrium position, and the bow planes were 
limited to approximately 15 degrees up and down angle. To 
effect such a restriction in ADS, the side constraint 
approach must be invoked. In Vanderplaats' formulation of 
the problem for the ADS package, the side constraints are 
the upper and lower bounds of the design variables to be 
optimized [Ref.20]. They are identified as VLB, VUB 
respectively in the programs. This is an advantage to a 
constrained optimization problem because it limits’ the 
search area in which to minimize the objective function. 
This kind of constraint can be bumped up against and become 
active, which means that the constraint value is in effect 
for a specific design variable. If all of the side 
constraints are active, then there is a good possibility 
that the true minimum has not been reached as the design 
variables have most likely hit ‘the constraint before the 


gradients had zeroed (found their minimum). If the side 
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constraints are active then the problem may need to be 
redefined within the limits of the side constraints. 
2. Quadratic Performance Index Weighting Alone 

The quadratic performance index (Eq. 3-7) was the 
objective function that ADS minimized using the 
unconstrained minimization techinque concluded from the 
previous work in Chapter II. The method, repeated here for 
the reader, was the Fletcher-Reeves optimizer (IOPT = 1) and 
the Polynomial extrapolation one dimensional search method 
(IONED = 4). 

The run times were favorable at less than four 
seconds for the weighting matrix determined "best." It was 
observed that as weighting was added to improve the depth 
accuracy the pitch accuracy worsened, and vice versa. As a 
result of this, the weighting matrix that was determined to 
produce the "best" result was the identity matrix. The 
"best" result produced a five percent difference between the 
desired depth and the actual depth. The pitch angle, 
however, ended up short by over six degrees although it was 
falling off toward zero from its maximum deflection. 

The results indicate, and are verified by another 
deeper depth change for the same terminal time, that 
although depth can be achieved relatively accurately the 
additional desired end state of zero pitch angle was not 
achievable. Figures 3.1 and 3.2 present the results of two 


different depth changes using the same gain matrix 
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determined for the first depth change (the deeper depth). 
The accuracy of the terminal depth, in general, increased 
with an increase in size of the depth change. 

The same depth change was then given twice as much 
time to get to the desired depth to test the adequacy of the 
weighting matrix to varying dive durations. Figures 3.3 and 
3.4 show the results of thesSe runs. The figures show that 
depth was increasing at the terminal time and pitch angle 
was falling off, but at an extremely slow rate. Figures 3.1 
throuyzn 3.4 illustrate that the "best" weighting was a 
function of the specific dive ordered, in the unconstrained 
end time cases. Clearly the two point boundary value 
problem could not be strictly enforced using this approach 
alone. It was obvious that something was needed to "drive" 
the solution to the desired terminal end states at the 
desired end time. 

3. ADS End State Constraints 

The choice of end constraint types available in ADS 
are found in the definition of the vector, IDG. IDG 
contains parameters that must be specified in the call to 
ADS and it specifies the type of each constraint used in the 
problem. Table 6 lists the available types of constraints 
handled by ADS ([Ref.20:p. 11}. Further investigation 
revealed that the best method to satisfy the end condition 
constraints was to use two ADS constraints defined as 


nonlinear equality types (IDG = -1) for each terminal end 
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TABLE 6 


CONSTRAINT TYPES IN ADS 


1. Linear equality (IDG(I) = -2) 
2. Nonlinear equality (IDG(I) = -1) 
3. Nonlinear inequality (IDG(I) = 0 or 1) 
4. Linear inequality (IDG(I) = 2) 
condition specified; the constraint and its negative. For 


example, a depth maneuver to an arbitrary depth of 50 feet 
required two end constraints to be written. ADS requires 


one of these to be entered via the following inequality: 


Gti) < 2 - 50. (3-8) 


Here, ADS was told to recognize the equality via IDG = -1. 
Thus when z is greater than 50 feet, this condition is not 
satisfied and it does penalize the optimization. Similarly, 


introducing the negative of this constraint, 

G(2) < 50 - 2 (3-9) 
enforces that Z must be less than or equal to 50 feet. ADS 
attempts to satisfy both constraints on Z by forcing the 


value of Z to 50 feet. A Similar method was used to apply 


terminal constraints on all the final states. 


3. 


In addition to the constraint relationships, the 
optimization algorithm had to be changed in order to carry 
out a constrained minimization problem. It was necessary to 
conduct a study of the effects on accuracy and run time, as 
well as a study on which optimization algorithm was best 
suited to this problem. 

The ADS choices for such a problem as this were 
(refer to Table 5) the 047, 057, 657, 533, and 133 options. 
Table 7 presents the results of running this ADS constrained 
terminal condition problem using each of the five options. 
By invoking the use of a strategy option, the run time 
increased a minimum of 66 percent over the shortest runtime. 
The "best" method, considering both accuracy and runtime, 
was the 057 option. This was used in all the remaining ADS 
constrained cases. 

Solving the terminal constrained problem using the 
057 option did require some weighting of the constraints in 
order to produce the "best" accuracy and run time. The 
weighting of the constraints was determined to be 0.5 for 
the two depth constraints and 10.0 for the two pitch 
constraints. When these weights were applied to a deeper 
depth maneuver, the final depth was approximately six inches 
from the desired depth. The pitch angle was also small, 
less than 0.02 degrees. Figures 3.5 and 3.6 present the 
results graphically for both depth and pitch, and for 


both depth changes. The results illustrate the versatility 


ore 


eo Sia 


98° 8 


SSS 


oe mre 


One corel 


Oe 


ArooGn 


eo 


t 


e 


_OTx 96°T 


S 


SSNS IL 


O09 ss 


Ss 


_OTx 9°€ 


_OTx p°9 


Lex Gor = 


oa 


e 


C 


OT 


ZTOL*T Wer LT 
b6°H on 
T6T Ziel 

SOT 09° 1 en Oras 

_OTx €6°2 ,_OTx €6°2 

_OTx S°S- TEZO°- 

0000°T- Z000°T- 
LS9 LS0 


SLTNSAHY SNOLTLdO Sav 


L Ald 


LLS9°T Cqo 
Tiaaer (98s) oUTZ UNY 

Col SUNIDY 
pO eel o3e21 yydep 
701% 62°E a3zer yo utd 
oS yo4td 
G6S66°- yydep 
Lv0 uot3do 


323 


yYoytTd pue yydaq yyoq uO SzUTeIAQSUOD Say butsn sz tTnseay yuadeq <s°¢€ aanbhty 


AWED 


y 





“avon ICC0. | 
* 1=dAdd 10 as 
} TVOO deaddudo 
‘ (INADA | S 
\ ia 
:  Saetin 


fa 


—_— oe es eee ee ee ee iia iil — meme eee i i i le 


34 


VI 


al 


G°€ amnbtd ut burmMoAnoueny ae 7o]q Wear 


CLIN EL 
Or Q 9 p 


g°¢ aanbty 





GI- O@- SZ- Of- SE- OF GF- 
HOLId 


OT- 


a 


of the ADS constraints to handle the typical range of depth 
changes without changing the weighting factors either on the 
constraints or within the objective function. 
4. ADS and Penalty Function Combinations 

Another method to enforce terminal conditions is by 
creating penalty functions and adding them to the original 
objective function [Ref. 19:p. 315]. Given the terminal 
conditions for this problem, the penalty function for depth 
and pitch respectively are, (ORDERED DEPTH - Z) squared and 
(PITCH) squared. Two cases were considered, the first case 
was an ADS constraint on pitch and the penalty function on 
depth. The second was an ADS constraint on depth and a 
penalty function on pitch. 

a. ADS Constraint on Pitch and Penalty on Depth 

This first case required both the pitch 

constraint and the depth penalty function to be weighted 
close to a thousand in order to achieve’ satisfactory 
accuracy for the 17 foot dive maneuver. Figures 3.7 and 3.8 
illustrate the trajectories and pitch performance of the two 
dives discussed. Weighting again appeared to be a function 
of the magnitude of the depth change maneuver using this 
combination to enforce the terminal conditions. 

b. ADS Constraint on Depth and Penalty on Pitch 

Reversing the roles of depth and pitch terminal 

constraints resulted in much improved accuracies’ and 


response then the previously discussed method. With the ADS 
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depth constraints weighted at unity and the pitch penalty 
function weighted at 70.0 for the 17 foot dive, the depth 
was perfectly satisfied and the pitch angle was extremely 
accurate. The performance of this combination when applied 
to the 100 feet depth change maneuver was considered 
acceptable. Weighting was not a function of the magnitude 
of bia depth change. Figures 3.9 and 3.10 graphically 
present the results. 
5. Unconstrained Minimization Using Both Penalties 

The final minimization method studied went back to 
using the unconstrained minimization technique and applied 
both of the terminal conditions as penalties on the 
objective function. Depth and pitch terminal conditions 
were put in penalty functions previously described and added 
external to the integral of the original objective function. 
No ADS defined constraints were used and ADS optimization 
combination 014 was applied as before. This method proved 
to be ill-fated for this objective function. Increased 
weighting on the penalties drove the answer to zero and the 
"best" weighting proved to be the identity matrix. However, 
at this weighting, the depth achieved was a third of the 
ordered depth but the pitch was acceptable at 0.0713 
degrees. For these reasons the data is not displayed 


graphically and this method was not further analyzed. 
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6. Timing and Accuracy 


Thus, only three of the methods were compared to 
choose the method best suited for this problem and the 
obstacle avoidance situation discussed in the next 
chapter. The three methods were: ADS defined constraints 
only, ADS defined constraint on pitch and penalty on depth 
and ADS constraint on depth and penalty on pitch. The 
results are compiled in Table 8. 

The timing criteria together with desirable accuracy 
caused the "ADS constraints only" method to be the best 
solution for the path planning problem posed as a two point 
boundary value problem. 

Although the deeper dives' final depth was off by 
Six inches in the selected method and pitch was slightly 
over a tenth of a degree, it was clear that this method had 
very good accuracy and near real time performance, it 
produced the smallest objective function and required the 
least reruns on the average than the other methods. 

It is evident that for the type of dynamic path 
planning problem undertaken here, the need for an end 
constraint to "drive" the trajectory to its final goal was 
determined. The ability of the ADS optimization program 
proved feasible to satisfy this simple linearized trajectory 


problem. 
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IV. PATH PLANNING APPLICATIONS 


A full scale nonlinear simulation model of the study 
vehicle was developed by Lt. Boncal in Reference 18. The 
model was a full six degree-of-freedom model with 12 states 
that took into account all properties of the vehicle and its 
motion through water. Comparison of the previous linear 
model results with those of the nonlinear model were made 
for the following reasons: 

1) the solution to the linear problem would be easier 
and, therefore, would require less real time to 
compute, and 

2) a determination was necessary as to whether the 
linearized version would be accurate enough compared 
to the nonlinear version. 

A. LINEAR VERSUS NONLINEAR PATH PLANNING 


Each time the optimization program was run, the data 


that it produced provided the control vector for each 


control surface used, e.g., the stern plane and bow plane 
time histories. Also, the time history of the states from 
beginning to end were computed. It was planned that this 


state time history would be the desired state trajectory 
which would be supplied to a state servo for the vehicle. 
Figure 4.1 illustrates this design concept. The servo then 
would follow the desired state and keep the error minimal 


between the actual position and the desired position, 
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Path 
Planner 






Controller 








Feedback 


X* optimum state vector 


Figure 4.1 Path Planner and Controller Relationship 


therefore following the optimal path. Consequently, the 
mecuracy of the X hastery is of Erisical importance. 

With the assumptions used in the simplified linear 
model,it was obvious that the straight ahead motion (the x 
position) would not correspond well to the nonlinear model. 
Since the depth was a function of the horizontal speed, the 
Z-position states could be expected to be in error as well. 
These results can be seen graphically in Figure 4.2 where 
both the nonlinear and linear optimized trajectories are 
represented in the x-z plane. Due to the decoupled axial 
force equation (X), the linear trajectory extends to the 
eal cyinite of the nonlinear trajectory. Due to the 
nonlinearities in the nonlinear model, the vehicle descends 
more rapidly, then levels out for a longer period of time. 
The linear model tends to make a more gradual descent and 
spend less time leveling out. 

The time to achieve the nonlinear optimal trajectory was 


an order of magnitude greater than for the linearized model 
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Figure 4.2 


using the same weighting factors for both models. This 
greatly hinders the feasibility of utilizing the full non- 
linear model for real time path planning. Since the 
nonlinear model was an entirely new plant, the analysis 
discussed previously for the linear model would have to be 
applied to improve the timing and accuracy in the nonlinear 
case. A study was conducted on the effect of weighting the 
ADS constraints only and an improvement from 60 seconds to 
approximately 22 seconds was achieved. Additional time may 
be saved if the best Q matrix could be determined as well. 
One additional test was run on the comparison of the two 
models. As mentioned earlier, the ADSL program also 
produces the control vector that achieved the optimized 
states. In this test, the control vector for the linearized 
model (Fig. 4.3a) was supplied to the nonlinear simulation. 
The results showed that the nonlinear model descended to 
within .05 feet of the ordered depth and the pitch was 
slightly over three tenths of a degree at the final time 
erg.) 4.3). Recalling that, although the states do not 
match up well enough to use the simplified linear model due 
to the oversimplification of the decoupled axial force 
equation (Fig. 4.2), the essential dynamics in the linear 
model were nevertheless valid since the desired depth was 


achieved using the combination above. 
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Figure 4.3 


B. OBSTACLE AVOIDANCE 

A primary goal of a path planner is to plan a safe 
trajectory between two points specified by the command 
intelligence of the vehicle. Between the two points may be 
visible obstacles, as well as hidden obstacles that may be 


unknown until the vehicle commences further on ites 


trajectory. Consequently, the safe path must sometimes be 
recomputed in real time to avoid a collision. Figure 4.4 
illustrates this with two trajectories. The first 


trajectory (called the "nonlinear trajectory") was the 
optimal or "best" path to the new depth without any 
obstacles in the path. With an obstacle positioned at the 
(+) symbol, the obstacle avoidance algorithm must compute a 
new trajectory for the vehicle to take to avoid the 
obstacle. The obstacle in this study was considered to be a 
fixed obstacle in space. 
1. Obstacle Avoidance Algorithm 

A simple obstacle avoidance algorithm was written 
for the purpose of testing the ADSL program to ensure its 
capability was adequate to perform this function. 

The algorithm was based on the procedure a human 
would use to avoid an obstacle in his path. Once the 
obstacle was detected, interest in the obstacle would 
heighten. The distance between the individual and the 
obstacle would be kept track of, and the human would take 


appropriate action to avoid, or make the decision to 
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Figure 4.4 


collide, and pay the penalty. Once past, interest in the 
object would decrease to Zero. In an AUV, collision walea 
the obstacle will not be an option. 

The size and position of any obstacle was determined 
to be information supplied by the command intelligence of 
the vehicle to the path planner. The command intelligence 
also was assumed to generate an area surrounding the 
obstacle that was termed the "avoidance zone." This zone 
was based on the size and maneuverability of the vehicle and 
the threat of the obstacle. This zone was what the path 
planner saw as the obstacle which the trajectory of the AUV 
center of gravity could not penetrate. For the purposes of 
this study, this obstacle data was emulated by the 
programmer who picked an arbitrary radius around a selected 
point obstacle. This is illustrated in Figure 4.5. Also 
for purposes of the present study, a one foot radius was 
arbitrarily chosen. The avoidance zone appears oval in the 
figure due to the axis scaling. 

The algorithm, which can be seen in the THE1ND DSL 
program in Appendix B, takes the time from start until the 
vehicle would be at the x-position of the obstacle and 
divides that equally into 10 positions with respect to time. 
As the positions are reached at the specified times, the 
distance between the vehicles' current position and the 
obstacle was computed. The distance then was made into an 


inequality constraint using ADS and defining IDG = 0 (refer 
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to Table 6). Each of these ten new constraints had to be 
satisfied by ADS. Figure 4.6 illustrates the results using 
the full scale nonlinear model. The obstacle was avoided 
and the avoidance zone was not violated by the vehicle 
trajectory. 

This simple algorithm only concerned itself with the 
obstacle up to the point where it was at the top center of 
the avoidance zone. This point may not always be the 
closest point of approach depending on the size of the 
avoidance zone. In these cases, the algorithm could be 
changed to compute distances to the obstacle for some 
designated amount of time after passing it. 

The linear and nonlinear obstacle avoidance runs 
were timed to determine the time difference between the 
obstructed and unobstructed run times. Table 9 presents the 
results. It can be seen that the linear model run time was 
increased four tenths of a second from the unobstructed case 
to the obstructed case. This has a great advantage, again, 
over the nonlinear case due to the fact that the nonlinear 


problem doubled its run time in order to avoid the obstacle. 
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TABLE 9 


LINEAR/NONLINEAR OBSTACLE AVOIDANCE TIMES 


MODEL TYPr WITHOUT OBSTACLE WITH OBSTACLE 
LINEAR 3.12958 C 3.50 SEE 
NONLINEAR 22007 JOES 48.67 SEC 


2. Trajectory Analysis 


Figure 4.3 shows the obstacle avoidance path above 
the optimal trajectory. This indicates that there are 
limits to the vehicle trajectory due to the vehicle dynamics 
and motion restrictions placed on the control surfaces. 
This effect was found when an obstacle was placed at 
adifferent points along the optimal path and the avoidance 
trajectory was always above the obstacle. This is true 
Since the ADS end constraints that are "driving" the vehicle 
are attempting to satisfy both of the weighted depth and 
pitch constraints. With an obstacle along the optimum path, 
the vehicles' maneuverability characteristics come into play 
--it becomes harder to take pitch off than it was to put it 


on to achieve the desired end state. 
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A. 


V. CONCLUSIONS AND RECOMMENDATIONS 


CONCLUSIONS 


The following conclusions can be drawn from. the 


feasibility study: 


1. 


Optimal control theory is a feasible method of path 
planning with or without fixed obstacles. This 
study demonstrated successful path planning using 
the following: 


a) the objective function 


ik 
f ({x¢(t)-x(t) TOLx- (t)-x(t)] + u(t) TRu(t) dt , 


0 


b) ADS equality constraints on desired end states, 
c) ADS inequality constraints for obstacle avoidance, 


qd) a fixed step integration method (Rectangular) to 
integrate the rate equations and the objective 
function (time step = 0.1 sec, 10 intervals). 


Real time path planning may be achieved with linear 
models. "Real time" here is arbitrarily determined to 
be from 1 to 2 minutes for the full 12 state 
optimization. Additional timing studies need to be 
done in order to determine this more precisely. The 
linearized dive plane equations of motion were studied 
and it was determined that a solution for a 20 second 
dive maneuver required three seconds of computer time 
to produce the "best" path. 


The full nonlinear model computation time for the same 
linear dive maneuver was determined to be at most 22 
seconds. 


The simplified linear state trajectory was not 
compatible with the nonlinear trajectory. The linear 
model for dive plane motion exceeded the realm of 
small perturbation theory enough to be considered as a 
very poor substitute for planning the path of a real 


a] 


vehicle in the dive plane under the assumption of 
decoupled dive plane motion. 


The ADS optimization methods that gave the best 
results in the unconstrained cases was the 
Fletcher-Reeves method (014) and for the constrained 
cases was the modified method of feasible directions 
O57 i. Introduction of the strategy option provided 
less accuracy and more run time in the unconstrained 
case, and good accuracy and additional time in the 
constrained case. 


The best method of achieving the trajectory between 
the start point and terminal point was to utilize the 
power of the ADS equality constraints to "drive" the 
trajectory to the desired final condition states. 
Weighting improved not only the accuracy but also run 
times. 


The weighting values of the constraints in the linear 
model were not the "“best"' combination for the 
nonlinear model. Weighting was determined to be a 
function more of the plant model used than the size of 
the maneuvers the plant must undergo when using ADS 
constraints. 


Obstacle avoidance was performed successfully using a 
simple algorithm that enforced constraints on the 
distance between the vehicle center of gravity and a 
given radius around the_- obstacle, called the 
"avoidance zone." 


RECOMMENDATIONS 


The following recommendations are offered to ‘continue 


the development of a path planner algorithm for use in an 


autonomous underwater vehicle: 


Jue 


Carry out the procedures implemented here on such 
additional models as: 


a) the full nonlinear model to simulate other vehicle 
motion than the dive plane alone; 


b) the full linearized model in other motions, 


c) nonlinear reduced order models of the vehicle. 
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Conduct model sensitivity studies on the limits of 
maneuverability of the vehicle. 


Continue development of the obstacle avoidance 
function to be able to have the model traverse a more 
dense environment of fixed and moving obstacles. 


Develop the interface between the AUV command and 
control intelligence and the path planner, and between 
the vehicle controller and path planner. 


Develop the program for eventual programming into a 
small microprocessor-based control structure. 
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APPENDIX A 


EXAMPLE 10.2-6 FROM SAGE [REF. 19:P. 313] 


Note: Solution presented here is based on the gradient in 
function space technique. 

Consider the minimization of J = = i (x2 + u*)dt for the 
system x = -x? + u, x(0) = 10.0. We first need to determine 
the adjoint and the gradient equations. For this problen, 
the Hamiltonian is H = =x? “+ su? - A\x2 + Au, and thus the 
adjoint equation is } = -x + 2\x with the terminal condition 
1(1) = O. The control gradient is 9d)H/du =u +2. Suppose 
that we guess the initial control u°(t) = 0, which ise 
too unreasonable, since the final value of the control, 
u(1), should be zero, and use K = 1. To implement the 


gradient method, the steps we must take are: 


1. Determine xN(t) from uN(t) for t «< [0,1] by 
xN = -(xN(t) )2 + uN (ty), xN (0) = 10 
2. Determine AN(t) from xN(t) by 


AN = -yN(ty + 2NetyxNce) , AN(1) = 0 
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3. Determine 3H/3uN from 


= uN(t) + AN(t) 
du 


4. Determine AuN(t) anda aJN from 


AuN(t) = -K sy = 7 KuN(t) - KAN (+t) 


2 a D 
] 


l 
fee aen (es ae =k f tut) + A(t) I~ at 
0 0 


aT 


5. Compute the control for the next iteration 
uNtleey) = uN(ty) + AuN(t) 


6. Shuffle data and repeat the procedure, starting at l, 
until auN(t) or ATN(t) changes very little from 
iteration to iteration. 

For the particular initial control assumed here, we 
obtain for the various steps in the procedure: 


mm saree = 0, Y-Ge) = 10/(1 + HOC) 


0 
2. AP(t) =i - (1 + 10t)?#/121] 


3. 9H/3u° =5{1 - (1 + 10t)2/121) 


A tee) - S[1 - (1 + 10t)*/121] 


1 
- . J (rl - (1 + 10t)2/121)}2dat 


A J°(t) 


- (0.0458) 
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1 
5. ul(t) = u%(t) + duS(t) = Sea Geom aeaee 
Figures 2.1 and 2.2 are the curves generated by the 


equations above. 
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APPENDIX B 


PROGRAMS 


This appendix contains the three primary programs thatwere 
used for this feasibility study. They are: 


1) STSPACE3 DSL--This is the state formulated DSL simulation 
of the linearized model for motion in the vertical plane. 


2) THEIND DSL--This is the ADSL program used to optimize the 
linear plant trajectories. 


3) OPTT3 DSL--This program is the ADSL program used to 
optimize the full scale nonlinear model with the stern and 
bow planes decoupled. 


PROGRAM NAME: STSPACE3 DSL 


TITLE AUV VERTICAL PLANE MANEUVERS (STATE SPACE) REVISED 

* 

* EQUILIBRIUM CONDITION IS CONSTANT SPEED (FT/SEC) IN 
* THE HORIZONTAL DIRECTION 

* 


CONST U0O=6.0 
‘ 


CONST MA= , THETAO= , WO= , IY= 

* 

CONST ZW=- , Z2Q= , ZQDOT= , ZWDOT= 
CONST ZDB= , ZDS= , ZO= 

* 

CONST MW= , MQ= , MQDOT= , MWDOT= 
CONST MDB= , MDS= , MTHETA= 


* 


METHOD RECT 
CONTROL FINTIM 


10., DELT= .1 


INITIAL 
Al=-ZW 
A2=(MA-ZWDOT) 
A3=-(ZQ+MA) 
A4=-ZQDOT 
B1=-MW 
B2=-MWDOT 
B3=-MQ 
B4=(IY-MQDOT) 
B5=-MTHETA 
C1=ZDS 
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C2=ZDB 
C3=MDS 
C4=MDB 
C5=C3-(B4*C1) /A4 
C6=C4-(B4*C2) /A4 
D1=B2-(B4*A2) /A4 
D2=B1-(B4*A1) /A4 
D3=B3-(B4*A3) /A4 
* 


DERIVATIVE 

* 
THETDD=(1/A4) * (C1*DS+C2*DB-Al*W-A2 *WDOT-A3 *THETAD) 
WDOT= (1/D1)*(C5*DS + C6*DB=- D2*W - D3*THETAD 
B5*THETA) 


THETAD= INTGRL(THETAO, THETDD) 
THETA = INTGRL(THETAO, THETAD) 
W = INTGRL(WO,WDOT) 

Z = INTGRL(ZO,W-(UO/UO) *THETA) 
DEPTH=-Z 

PITCH=THETA/ .01745 
BOWANG=(DB/.01745) 


STNANG=(DS/.01745) 
* 


DYNAMIC 
P 4 
DS = .08725*STEP(0.0)-.02843*STEP(2.0)-... 
.003636*STEP (3.0) -.008036*STEP(4.0)-... 
.015477*STEP(5.0) -—.02670*STEP(6.0)-... 
.04007*STEP(7.0) -.053332*STEP(8.0)- 
.086084*STEP (9.0) 
DB = -.2443*STEP(0.0) + .3186*STEP(8.0) + 
.17*STEP (9.0) 
* 
TERMINAL 


* 


PRINT 1.,THETDD, THETAD, THETA, ZDD,W,Z,DEPTH, PITCH,DS... 
, BOWANG, STNANG 

SAVE .5,THETDD, THETAD, THETA, ZDD,W,Z,DEPTH,PITCH,DS... 
, BOWANG, STNANG 

GRAPH (DE=TEK618) TIME,DS 

GRAPH (DE=TEK618) TIME, DEPTH 

GRAPH (DE=TEK618) TIME, WDOT 

GRAPH (DE=TEK618) TIME,W 

GRAPH (DE=TEK618) TIME, THETDD 

GRAPH (DE=TEK618) TIME, THETAD 

GRAPH (DE=TEK618) TIME, THETA 

GRAPH (DE=TEK618) TIME, PITCH 

GRAPH (DE=TEK618) TIME, BOWANG 

GRAPH (DE=TEK618) TIME,STNANG 

END 

STOP 
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NOTE: Coefficients must be supplied for the PataelculLar 
vehicle being studied. 
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PROGRAM NAME: THE IND Dosis 


TITLE RUN: (NR) LINEAR AUV DYNAMIC PATH PLANNER FOR VERTICAL 
PLANE MOTION 

SEPARATED BOW AND STERN PLANE CONTROL NON-DIMENSIONAL 

DATA FOR NEW OBJECTIVE THAT INCLUDES THE ERROR UNWEIGHTED 

UPDATE: **OBSTACLE AVOIDANCE** 

USING OBJ= INTGRL? (Z-10) **2+W**2+THETA**2+THETAD* *2+U#*2 

OBJECTIVE FUNCTION WITH ADS CONSTRAINTS 

kkk kkkkkkkkkkkkkekekekekeee ADSL SET [Pexx kek eek kkkkkekeak 

* 

FIXED ISTRAT, IOPT, IONED, IPRINT, INFO, IGRAD, NDV, NCON 

FIXED IDG, NGT, IC, NRA, NCOLA, NRWK, IWK, NRIWK, 0, H 

D DIMENSION AW(42, 42) 

ARRAY WK(5000), IWK(1000) ,DIST(15) 

ARRAY DX(21),VLB(21) ,VUB(21) ,GW(15), DF(21), IDG(15), IC(20) 

PARAM NRA=42, NCOLA=42, NRWK=5000, NRIWK=1000 

PARAM IGRAD=0, INFO=0, NDV=20, NCON=15, NGT=20 

TABLE DX(1-2)=2*.0, DX(3-21)=19*0., IDG(1-4)=4*-1 

TABLE IDG(5-15)=11*0 

TABLE VLB(1-9)=10*-.17452, VLB(11-19)=9*-.2443 

TABLE VLB(10)=0.,VLB(20-21)=0. 

TABLE VUB(1-9)=9*.17452, VUB(11-19)=9*.2443 

TABLE VUB(10)=0.,VUB(20-21)=0. 

TABLE DIST(15)=15*0.0 : 

PARAM ISTRAT=0, IOPT=5, IONED=7, IPRINT=2020 

INCON U=0., H=0 

METHOD RECT 

CONTROL FINTIM= 7.00, DELT=.1 


PRINT THETAD,W, DEPTH, PITCH, XPOS, ZPOS, DT 
* 


kh kkk kk RK KK KK KKK KKEKEEEEEEKDSI, MODEL SET UPkx kkk eek keke 
. 


+ + + * * 


« EQUILIBRIUM CONDITION IS CONSTANT SPEED (NON- 
* DIMENSIONALIZED) BY UO = 6 FT/SEC 

* 

CONST UO=1.0, XOBS= , ZOBS= 


* 


CONST MA=0.0962, THETAO=0., ZO=0.,WO=0., IY=.00606 
x 


CONST ZW= , 2Q= , ZQDOT= , ZWDOT= 
CONST ZDB= , Z2DS= , ZO=0. 

= 

CONST MW= , MQ= , MQDOT= , MWDOT= 
CONST MDB= , MDS= , MTHETA= 

* 

* 

INITIAL 


* 
ORDDEP = 1.00 
Al=-ZW 
A2=(MA-ZWDOT) 
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* 


A3=-(ZQ+MA) 

A4=-ZQDOT 

B1=-MW 

B2=-MWDOT 

B3=-MQ 

B4=(IY-MQDOT) 

B5=-MTHETA 

C1=ZDS 

C2=ZDB 

C3=MDS 

C4=MDB 

C5=C3-(B4*C1) /A4 

C6=C4-(B4*C2) /A4 

D1=B2-(B4*A2) /A4 

D2=B1-(B4*A1) /A4 

D3=B3-(B4*A3) /A4 

CALL DADS(INFO, ISTRAT, IOPT,IONED,IPRINT,IGRAD,... 
NDV,NCON, DX, VLB, VUB,OBJ,GW,IDG,NGT,IC,DF,... 
AW, NRA, NCOLA, WK, NRWK, IWK, NRIWK) 

IF(INFO.EQ.0) DELPRT = 0.2 

IF(INFO.EQ.0) DELPLT = 0.2 


DERIVATIVE 


* 


* 


THETDD=(1/A4) * (C1*DS+C2*DB-A1*W-A2 *WDOT-A3 *THETAD) 

WDOT= (1/D1)*(C5*DS + C6*DB - D2*W - D3*THETAD - 
B5*THETA) 

THETAD= INTGRL(THETAO, THETDD) 

THETA = INTGRL(THETAO, THETAD) 

W'= INTGRL(WO,WDOT) 

Z = INTGRL(ZO,W-UO*THETA) 

DEPTH=-Z 

PITCH=THETA/.01745 

BOWANG=(DB/.01745) 

STNANG=(DS/.01745) 

INTGRD = ((W*W+(Z-ORDDEP) *(Z-ORDDEP)+... 

THETAD*THETAD+THETA*THETA) ) + (DS*DS+DB*DB) 
OBJ1 = INTGRL(0., (0.5) *INTGRD) 
OBJ = OBJ1 


DYNAMIC 


* 


+e + H+ + 


RN=TIME/ (FINTIM/10.-DELT/10000.) 
O=INT (RN) +1 
IF(O.EQ.11) O=10 


ADDITIONALLY THE PLANES SHOULD BE AT EQUILIBRIUM SO THE 
VEHICLE WILL PROCEED AT THIS NEW DEPTH WITHIN SOME 
TOLERANCE 

DS=DX (0) 


DB=DX (10+0) 
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+ + + 


+ + 


IF(O.GE.10) DS=0. 
IF(O.GE.10) DB=0. 


CONSTRAINTS FOR A DIVE 


ORDERED DEPTH = ORDDEP 


GW(1) =(Z-ORDDEP) /2. 
GW(2) =(ORDDEP-Z) /2. 


AUV'S FINAL STATE MUST BE LEVEL FLIGHT AS FOLLOWS 


GW (3) 
GW (4) 


THETA*10. 
=THEDA=0- 


* A-Z POSITIONING FOR OBSTACLE AVOIDANCE 


* 


XPOS=17.425*TIME 
ZPOS=-2Z*17.425 


AVOIDING THE OBSTACLE 

IF (TIME.GE.0.0.AND.TIME. LE. XOBS/17.425) THEN 

TIME1 = XOBS/17.425 

QN=TIME/ (TIME1/10.-DELT/10000. ) 

D=INT(QN) #1 

DIST (D) =SQRT( (XPOS-XOBS) * (XKPOS-XOBS) +(ZPOS-ZOBS) *... 
(ZPOS-ZOBS)) — 

GW(4+D) = (1.-DIST(D)) 

ELSE 

ENDIF 


TERMINAL 


9999 
9000 


* 


END 
STOP 


NOTE: 


IF(INFO.EQ.0) THEN 
DO 9000 D=1,11 
WRITE(*,9999) DIST(D) 


FORMAT (1X,E15.4) 
CONTINUE 

ELSE 

ENDIF 

IF(INFO.EQ.0) CALL ENDJOB 
CALL RERUN 


The coefficients must be provided for the particular 
vehicle being studied. 
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PROGRAM NAME: OFa1S DSL 


TITLE RUN: NONLINEAR AUV MODEL / STERN PLANE AND BOW PLANE 
SEPARATED 
) UPDATED: 11/16/87 
) 100.00 FT DEPTH CHANGE IN 20 SEC 
3) RIGHT OBJ EQUATION 
) ADS CONSTRAINTS ON DEPTH AND PITCH 
) OBSTACLE FURTHER DOWN THE TRAJECTORY AND ABOVE IT 


+e + + HF 


kkkkkkkkkkekekeekeeREKKEKADSL SET-UPEk KKK KKK KKK KR KKK KKK KKK KK KK 


*FIXED ISTRAT, IOPT, IONED, IPRINT, INFO, IGRAD, NDV, NCON 
FIXED IDG, NGT, IC, NRA, NCOLA, NRWK, IWK, NRIWK, 0, H,D,c 
D DIMENSION AW(82,82) 

ARRAY WK(5000), IWK(1000) 

ARRAY DIST(40) 

ARRAY DX(21),VLB(21),VUB(21) ,GW(15), DF(21), IDG(15), IC(20) 
PARAM NRA=82, NCOLA=82, NRWK=5000, NRIWK=1000 

PARAM IGRAD=0, INFO=0, NDV=20, NCON=15, NGT=20 

TABLE DX(1-2)=2*.0,DX(3-21)=19*0., IDG(1-4)=4*-1 

TABLE IDG(5-15) =11*0 

TABLE VLB(1-9)=9*-.17452, VLB(11-19)=9*-.2443 

TABLE VLB(10)=0.,VLB(20-21)=0. 

TABLE VUB(1-9)=9*.17452, VUB(11-19) =9*.2443 

TABLE VUB(10)=0. ,VUB(20-21)=0.TABLE DIST(1-40)=40*0. 

PARAM ISTRAT=0, IOPT=5, IONED=7, IPRINT=2020 

INCON H=0, OBS1=0.,YZONE=0. 

METHOD RECT 

CONTROL FINTIM=20., DELT=.10 


PRINT THETAD,W, DEPTH, PITCH, XPOS, DEPTH,NDX,NDZ,NDT 
ke 


kkkkkkkkk keke RKRKEKEEKDSIL MODEL SET UDPkek kkk kK KKK KKK KKK KK KEK 
* 


D DIMENSION MM(6,6),G4(4) ,GK4(4) ,BR(4) ,HH(4) 


D DIMENSION B(6,6),BB(6,6) 

D DIMENSION A(12,12), AA(12,12) 

D COMMON / BLOCK1 / F(12), FP(6), MMINV(6,6), UCF(4) 
FIXED N,IA,IDGT,IER,LAST,J,K,M,JJ,KK,I 

INTEGER 


ARRAY WKAREA(54), X(12) 
* 


* 


CONST 

* 

“ LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 

* 

CONST XPP = ,xXQQ = ,XRR = ,XPR = 
XUDOT= ,XWQ = ,XVP = ,XVR = 
XQDS= , XQDB= , XRDR= ,XVV = 
XWW = ,XVDR= ,XWDS= , XWDB= 
XDSDS= ,XDBDB= , XDRDR= ,XQDSN= 
XWDSN= ,XDSDSN= 
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LATERAL HYDRODYNAMIC COEFFICIENTS 


CONST YPDOT= ,YRDOT= ,YPQ = ,YOR = 
YVDOT= ,YP = ANS = i 2 
YWP = ,YWR = YV = -YVW = 
YDR = ,CDY = 

* 

* NORMAL HYDRODYNAMIC COEFFICIENTS 

* 

CONST ZQDOT= ,2PP = ,2PR = ,oRR = 
ZWDOT= ,2Q = ,2VP = ,2VR = 
ZW = \ZVV = /ZDS = /ZDB = 
ZQN = ,2WN = , 2DSN= ,CDZ = 

* 

* ROLL HYDRODYNAMIC COEFFITIENTS 

* 

CONST KPDOT= ,KRDOT= ,KPQ = , KOR = 
KVDOT= ,KP = ,KR = ,KVQ= 
KWP =  ,KWR = /KV = KVW = 
KPN = ,KDB = 

* 

* PITCH HYDRODYNAMIC COEFFICIENTS 

* 

CONST MQDOT= ,MPP = /MPR = /MRR = 
MWDOT= »,MQ = »,MVP = »;MVR = 
MW = »>MVV = ,MDS = ,»MDB = 
MON = ,MWN = ,MDSN = 

* 

* YAW HYDRODYNAMIC COEFFICIENTS 

* 

CONST NPDOT= ,NRDOT= /NPQ = /NOR = 
NVDOT= ,NP = ,NR = ,;NVQ = 
NWP = ,;NWR = N= ,NVW = 
NDR = 

* 

x MASS CHARACTERISTICS OF THE FLOODED MARK IX VEHICLE 

* 

CONST WEIGHT = , BOY = ,VOL = .XG = 
YG = , 2G = ,sB = ,2B = 
IX = , LY = ,1Z = ,LAL = 
IYZ = , LXY = ,YB = 
L = , RHO = a= /NU = 
AO = ,KPROP = ,NPROP = ,X1TEST= 
DEGRUD= , DEGSTN= 

* J 

* INPUT INITIAL CONDITIONS HERE IF REQUIRED 

= 

INIT RAL 

NOSORT 
D=0 
H=H+1 
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+ 


+ 


ORDDEP = 


17.425 


XOBS=58. 666 
ZOBS=6.0000 
IF(H.EQ.1) THEN 


Dovuvsa ac 
lou te de wea 
oOoCOCOC°o 
200000 


wooodod0on 


ome, hy 
WWM ” 

-H 
| | oO 
oo ll 


ne 
lilovwo 


RPM = 
LATYAW 
NORPIT 


ollooooo0o°o 


e 


OOO 
© 


© 


0.0 
0.0 


RE = UO*L/NU 


CDO =. 


00385 + (1.296E-17)*(RE - 1.2E7) **2 


DEFINE LENGTH FRACTIONS FOR GAUSS QUADUTURE TERMS 


G4(1) 
G4 (2) 
G4 (3) 
G4(4) 


0.069431844 
0.330009478 
0.669990521 
0.930568155 


DEFINE WEIGHT FRACTIONS FOR GAUSS QUADUTURE TERMS 


GK4 (1) 
GK4 (2) 
GK4 (3) 
GK4 (4) 


DEFINE 
INTEGRAT 


0.1739274225687 
0.3260725774312 
0.3260725774312 
0.1739274225687 


THE BREADTH BB AND HEIGHT HH TERMS FOR THE 
ION 
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10 


+ + + & 


BR(1) = 75.7/12 
BR(2) = 75.7/12 
BR(3) = 75.7/12 
BR(4) = 55.08/12 
HH(1) = 16.38/12 
HH(2) = 31.85/12 
HH(3) = 31.85/12 
HH(4) = 23.76/12 


MASS = WEIGHT/G 


DIVAMP = DEGSTN*0.0174532925 
RUDAMP = DEGRUD*0.0174532925 
N = 6 
DO 15 J = 1,N 

DO 10 K = 1,N 


a 
MMINV(J,K) = 0.0 
MM(J,K) = 0.0 


CONTINUE 
CONTINUE 
MM(1,1) = MASS -((RHO/2) *(L**3) *XUDOT) 
MM(1,5) = MASS*ZG 
MM(1,6) = -MASS*YG 
MM(2,2) = MASS -((RHO/2) *(L**3) *YVDOT) 
MM(2,4) = -MASS*ZG -((RHO/2) *(L**4) *YPDOT) 
MM(2,6) = MASS*XG - ((RHO/2)*(L**4) *YRDOT) 
MM(3,3) = MASS - ((RHO/2) *(L**3) *ZWDOT) 
MM(3,4) = MASS*YG 
MM(3,5) = -MASS*XG -((RHO/2)*(L**4) *ZQDOT) 
MM(4,2) = -MASS*ZG - ((RHO/2) *(L**4) *KVDOT) 
MM(4,3) = MASS*YG 
MM(4,4) = IX - ((RHO/2)*(L**5) *KPDOT) 
MM(4,5) = -IXY 
MM(4,6) = -IXZ -((RHO/2) *(L**5) *KRDOT) 
MM(5,1) = MASS*ZG 
MM(5,3) = -MASS*XG -((RHO/2) *(L**4) *MWDOT) 
MM(5,4) = -IXY 
MM(5,5) = IY -((RHO/2) *(L**5) *MQDOT) 
MM(5,6) = -IYZ 
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MM (6,1) -MASS*YG 


MM(6,2) = MASS*XG -((RHO/2) *(L**4) *NVDOT) 
MM(6,4) = -IXZ - ((RHO/2) *(L**5) *NPDOT) 
MM(6,5) = -IYZ 
MM(6,6) = IZ - ((RHO/2) *(L**5) *NRDOT) 
* 
* 
LAST = N*N+3*N 
DO 20 M = 1,LAST 
WKAREA(M) = 0.0 
20 CONTINUE 
* 
IER = 0 
IA = 6 
IDGT = 4 


WE iE ( 8) 400) ((MM(1I,d)), J =e,6),1 = 1,56) 
CALL LINV2F(MM,N,1IA,MMINV, IDGT,WKAREA, IER) 


WRITE( 8,400) ((MMINV(I,J), J = 1,6),I = 1,6) 
400  FORMAT(6E12.4) 


EES 
ENDIF 


CALL DADS(INFO,ISTRAT,IOPT,IONED,IPRINT,IGRAD,... 
NDV,NCON, DX, VLB, VUB,OBJ,GW, IDG, NGT, IC, DF, AW,NRA,... 
NCOLA, WK, NRWK, IWK, NRIWK) 

IF(INFO.EQ.0) DELPRT = 1.0 

* 

DERIVATIVE 

NOSORT 

* 


* PROPULSION MODEL 
* 
SIGNU = 1.0 
IF (U.LT.0.0) SIGNU = -1.0 
IF (ABS(U).LT.X1TEST) U = X1TEST 
SIGNN = 1.0 
IF (RPM.LT.0.0) SIGNN = -1.0 
ETA = 0.012*RPM/U 
RE = U*L/NU 


CDO = .00385 + (1.296E-17)*(RE - 1.2E7)**2 
CT = 0.008*L**2*ETA*ABS (ETA) / (AO) 

CT1 = 0.008*L**2/(AO) 

EPS = -1.0+SIGNN/SIGNU*(SORT(CT+1.0)-... 


1.0) /(SQRT(CT1+1.0)-1.0) 
XPROP = CDO*(ETA*ABS(ETA) -1.0) 


* CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE 


18 


* VEHICLE AND INTEGRATE USING A 4 TERM GAUSS QUADUTURE 


LATYAW = 0.0 
NORPIT = 0.0 
DO 500 K = 1,4 

UCF(K) = SQRT((V+G4(K) *R*L)**2 + (W-G4(K) *Q*L) **2) 


IF (UCF(K).GT.1E-10) THEN 


TERMO = (RHO/2)*(CDY*HH(K) *(V+G4(K) *R*L)**2 +... 
CDZ*BR(K) * (W-G4 (K) *Q*L) **2) 

TERM1 = TERMO*(V+G4 (K) *R*L) /UCF(K) 
TERM2 = TERMO*(W-G4(K) *Q*L) /UCF(K) 
LATYAW = LATYAW + TERM1*GK4 (K) *L 
NORPIT = NORPIT + TERM2*GK4(K) *L 
END IF 

500 CONTINUE 

* 

* 

* FORCE EQUATIONS 

* 

*& 

‘ LONGITUDINAL FORCE 

* 


FP(1) =MASS*V*R-MASS*W*Q+MASS*XG*Q**2+MASS*KG*R**2—-.., 
MASS *YG*P*Q-MASS *ZG*P*R+ (RHO/2) *L**4* ae 
(XPP*P**2 + XQQ*Q**2 + XRR*R**2 + XPR*P*R)  ... 
+ (RHO/2) *L**3*(XWQ*W*Q +XVP*V*P+XVR*AV*R+URQ 2... 
* (XQDS*DS+XQDB*DB) +XRDR*U*R* DR)+ ae 
(RHO/2) *L*¥*2* (XVV¥VH*2 + XWWAWHE2 + - 
XVDR*U*V*DR + U*W* (XWDS*DS+XWDB*DB) +U**2* pie 
(XDSDS*DS**2+XDBDB*DB**2 + XDRDR*DR**2))- ‘lea 
(WEIGHT -BOY)*SIN(THETA) +(RHO/2) *L**3* cat 
XQDSN*U*Q*DS*EPS+ (RHO/2) *L**2* (XWDSN*U*W*DS+... 
XDSDSN*U**2*DS**2)*EPS +(RHO/2) *L**2*U**2*XPROP 


+ 


LATERAL FORCE 


-MASS*U*R + MASS*XG*P*Q + MASS*YG*R**2 - a. 
MASS*ZG*Q*R +(RHO/2) *L**4*(YPQ*P*Q + a 
YQR*Q*R) +(RHO/2) *L**3*(YP*U*P + YR*U*R + in 
YVO*V*Q + YWP*W*P + YWR*W*R) + (RHO/2)*L**2*... 
(YV*U*V + YVW*V*W +YDR*U**2*DR) -LATYAW = 
+(WEIGHT-BOY)*... COS(THETA) *SIN(PHI) 


FP(2) 


+ 


NORMAL FORCE 


FP(3) = MASS*U*Q - MASS*V*P - MASS*XG*P*R - see 
MASS*YG*Q*R +MASS*ZG*P**2 + MASS*ZG*Q**2 + ... 
(RHO/2) *L**4%*(ZPP*P**2 + ZPR*P*R +ZRR*R**2)... 
+ (RHO/2) *L**3*(ZQ*U*Q + ZVP*V*P + ZVR*V*R)... 
+(RHO/2) *L*¥*2*(ZWkUAW + ZVV*VE*2+U%*2* (ZDS*... 
DS+ZDB*DB) ) -NORPIT+ (WEIGHT-BOY) *COS (THETA) *... 
COS (PHI)+ (RHO/2) *L**3*ZQN*U*Q*EPS 2 
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+ (RHO/2) *L**2* (ZWN*U*W +ZDSN*U**2*DS) *EPS 
* ROLL FORCE 


FP(4) =-IZ*Q*R+IY*Q*R-IXY*P*R +1IYZ*Q*e*2 -LYZ*R**2 
+IXZ*P*Q +MASS*YGRU*Q -MASS*YG*V*P - 


MASS *ZG*W* P+ (RHO/2) *L**5%* (KPQ*P*Q+ KQR*Q*R)... 


+ (RHO/2) *L**4*(KP*U*P +KR*U*R + KVQ*V*Q + 
KWP*W*P + KWR*W*R) +(RHO/2) *L**3%*(KV*U*V + 
KVW*V*W) +(YG*WEIGHT -YB*BOY) *COS (THETA) * 
COS(PHI) -(ZG*WEIGHT -ZB*BOY) *COS(THETA) * 
SIN(PHI) + (RHO/2) *L**4*KPN*U*P*EPS+ 
(RHO/2) *L**3*U**2*KPROP +MASS*ZG*U*R 


* PITCH FORGE 


FP(5) = -IX*P*R +I1Z*P*R+IXY*Q*R-LYZ*P*Q-IXZ*PR*Q 
+IXZ*R**2 -MASS*XG*U*Q + MASS*XG*V*P + 
MASS*ZG*V*R -MASS*ZG*W*Q + 
(RHO/2) *L**5*(MPP*P**2 +MPR*P*R 


+MRR*R**2) + (RHO/2) *L*¥*4* (MQ*U*Q + MVP*#V*P +... 


MVR*V*R) + (RHO/2) *L**3*(MWkU*W + 
MVV*V**2+U**2* (MDS*DS+MDB*DB))+ NORPIT - 
(XG*WEIGHT- XB*BOY) *COS(THETA) *COS(PHI) + 
(RHO/2) *L**3* (MWN*U*W+MDSN*U**2*DS) *EPS- 
(ZG*WEIGHT-ZB*BOY) *SIN (THETA) 


* YAW FORCE 


FP(6) = -LY*P*Q+IX*P¥Q+IXY* PRX2—-ITXY*#QR*2+TYZ*P*R —-... 


IXZ*Q*R -MASS*XG*U*R + MASS*XG*W*P - 
MASS*YG*V*R + MASS*YG*W*Q + 

(RHO/2) *L**5* (NPQ*P*Q + NQR*Q*R) 

+ (RHO/2) *L**4* (NP*U*P+NR*U*R + NVQ*V*Q 


+NWP*W*P + NWR*W*R) +(RHO/2) *L**3%*(NV*URV +... 
NVW*V*W+NDR*U**2*DR)- LATYAW + (XG*WEIGHT -... 


XB*BOY) *COS (THETA) *SIN (PHI) +(YG*WEIGHT) * 
SIN(THE TA)+(RHO/2) *L**3*U**2 *NPROP- 


YB*BOY*SIN (THETA) 
* 
- NOW COMPUTE THE F(1-6) FUNCTIONS 
* 
DO 600 J = 1,6 
F(J) = 0.0 
DO 600 K = 1,6 
F(J) = MMINV(J,K)*FP(K) + F(d) 


600 CONTINUE 


THE LAST SIX EQUATIONS COME FROM THE KINEMATIC 
RELATIONS 


+ + + + 


FIRST SET THE DRIFT CURRENT VALUES 


oo 


+ 


UCO = 0.0 
VCO = 0.0 
WCO = 0.0 


INERTIAL POSITION RATES F(7-9) 


F(7) = UCO + U*COS(PSI) *COS(THETA) + 
V* (COS (PSI) *SIN (THETA) * 
SIN(PHI) - SIN(PSI)*COS(PHI)) + a 
W* (COS (PSI) *SIN(THETA) *COS (PHI) + ee 
SIN(PSI) *SIN(PHTI) ) 

F(8) VCO + U*SIN(PSI)*COS(THETA) + ee 

V* (SIN(PSI) *SIN(THETA) *SIN(PHI) + si 

COS (PSI) *COS (PHI) )+ W*(SIN(PSI) *SIN(THETA) *.. 

COS (PHI) -COS(PSI) *SIN(PHI) ) 


F(9) = WCO - U*SIN(THETA) +V*COS(THETA)*SIN(PHI) ... 
+W*COS (THETA) *COS (PHI) 


EULER ANGLE RATES F(10-12) 


F(10) = P + Q*SIN(PHI) *TAN (THETA) + - 
R*COS (PHI) * TAN (THETA) 
F(11) = Q*COS(PHI) - R*SIN(PHI) 


F(12) = Q*SIN(PHI)/COS(THETA) + R*COS(PHI) /COS (THETA) 


UDOT = F(1) 

VDOT = F(2) 

WDOT = F(3) 

PDOT = F(4) 

QDOT = F(5) 

RDOT = F(6) 

XDOT = F(7) 

YDOT = F(8) 

ZDOT = F(9) 

PHIDOT = F(10) 

THETAD = F(11) 

PSIDOT = F(12) 

U = INTGRL (UO,UDOT) 

V = INTGRL(0.0, VDOT) 

W = INTGRL(0.0,WDOT) 

P = INTGRL(0.0, PDOT) 

Q = INTGRL(0.0,QDOT) 

R = INTGRL(0.0,RDOT) 
XPOS = INTGRL(0.0,XDOT) 
YPOS = INTGRL(0.0,YDOT) 
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* 


ZPOS = INTGRL(0.0,ZDOT) 
PHI = INTGRL(0.0, PHIDOT) 
THETA = INTGRL(0.0,THETAD) 
PSI = INTGRL(0.0,PSIDOT) 


PHIANG = PHI/0.0174532925 
THEANG = THETA/0. 0174532925 
PSIANG = PSI/0.0174532925 


DEPTH=-ZPOS 
PITCH=THEANG 
BOWANG=(DB/.01745) 
STNANG=(DS/.01745) 
INTGRD = (U*U+V*V+W*W+P*P+Q*Q+R*R+XPOS*XPOS+YPOS*... 
YPOS+ (ZPOS-ORDDEP) * (ZPOS-ORDDEP) +PHI*PHI+... 
THETA*THETA+PSI*PSI) + (DS*DS+DB*DB) 
OBJ1 = INTGRL(0., (0.5) *INTGRD) 
OBJ = OBJ1 


DYNAMIC 


+ +e + + 


+ e+ +e He + 


+ 


RN=TIME/ (FINTIM/10.-DELT/10000.) 


O=INT (RN) +1 


IF(9.EQ.11) O=10 


ADDITIONALLY THE PLANES SHOULD BE AT EQUILIBRIUM SO THE 
VEHICLE WILL PROCEED AT THIS NEW DEPTH WITHIN SOME 


TOLERANCE 


DS=DX (0) 

DB=DxX (10+0) 
IF(O.GE.10) DS=0. 
IF(O.GE.10) DB=0. 
DR=DX (20+0) 
RPM=DX (30+0) 


CONSTRAINTS FOR A DIVE 


ORDERED DEPTH = ORDDEP 


GW(1) = (ZPOS-ORDDEP) /5. 
GW(2) = (ORDDEP-ZPOS)/5. 
AUV'S FINAL STATE MUST BE LEVEL FLIGHT AS FOLLOWS 
GW(3) = THETA/5. 
GW(4) = -THETA/5. 


AVOIDING THE OBSTACLE 


IF (TIME.GE.0.0.AND.TIME.LE.XOBS/U) THEN 
TIME] = XOBS/U 

QN=TIME/ (TIME1/10.-DELT/10000.) 
D=INT(QN) +1 

DIST (D) =SQRT ( (XPOS-XOBS) * (XPOS-XOBS) + (ZPOS- 
ZOBS) *(ZPOS-ZOBS)) GW(4+D) = (1.-DIST(D)) 


ie] 


ELSE 
ENDIF 
NDX=XPOS/17.425 
NDZ=-ZPOS/17.425 
NDT=TIME*6./17.425 
* 
TERMINAL 
* 
IF(INFO.EQ.0) THEN 
DO 9000 D=1,11 
WRITE (*,9999) DIST(D) 
9999 FORMAT(1X,E15.4) 
9000 CONTINUE 


ELSE 
ENDIF 
IF(INFO.EQ.0) CALL ENDJOB 
CALL RERUN 
* 
END 
STOP 


NOTE: The coefficients must be supplied for the particular 
vehicle being studied. 
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